#include "driver/gpio.h"
#include "led.h"

#include "../robots/motor_manage.h"
// #include "../robot.h"
// #include "../mlog/Mdebug.h"

/**********fly************** */
// #define LED_RED1_PIN    32
// #define LED_BLUE2_PIN   12
// #define LED_BLUE3_PIN   13
// #define LED_RED4_PIN    23

// static unsigned int led_pin[LED_NUM] = {
//     [LED_RED1] = LED_RED1_PIN,
//     [LED_BLUE2] = LED_BLUE2_PIN,
//     [LED_BLUE3] = LED_BLUE3_PIN,
//     [LED_RED4] = LED_RED4_PIN,
// };

/***********led************** */
#define LED_RED1_PIN    10


static unsigned int led_pin[LED_NUM] = {
    [LED0] = LED_RED1_PIN,
};

// save led state
static char led_state[LED_NUM];
static bool isInit = false;

//Initialize the green led pin as output
void ledInit(void)
{
    int i;
    if (isInit) {
        return;
    }

    // for (i = 0; i < LED_NUM; i++) {
    //     gpio_config_t io_conf = {
    //         //bit mask of the pins that you want to set,e.g.GPIO18/19
    //         .pin_bit_mask = (1ULL << led_pin[i]),
    //         //disable pull-down mode
    //         .pull_down_en = 0,
    //         //disable pull-up mode
    //         .pull_up_en = 0,
    //         //set as output mode
    //         .mode = GPIO_MODE_OUTPUT,
    //     };
    //     //configure GPIO with the given settings
    //     gpio_config(&io_conf);
    //     ledSet(i, LED_OFF);
    // }
            gpio_config_t io_conf = {
            //bit mask of the pins that you want to set,e.g.GPIO18/19
            .pin_bit_mask = (1ULL << LED_RED1_PIN),
            //disable pull-down mode
            .pull_down_en = 0,
            //disable pull-up mode
            .pull_up_en = 0,
            //set as output mode
            .mode = GPIO_MODE_OUTPUT,
        };
        //configure GPIO with the given settings
        gpio_config(&io_conf);
        for (i = 0; i < LED_NUM; i++) 
            ledSet(i, LED_OFF);

    isInit = true;
}

// bool ledTest(void)
// {
//     ledSet(LED_RED1, LED_OFF);
//     ledSet(LED_RED4, LED_ON);
//     ledSet(LED_BLUE2, LED_OFF);
//     ledSet(LED_BLUE3, LED_ON);
//     return isInit;
// }


void ledCtrolAll(LED_STATE value)
{
    int i;
    for (i = 0; i < LED_NUM; i++) {
        ledSet(i, value);
    }
}

// void ledSet(led_t led, LED_STATE value)
// {
//     if (led >= LED_NUM ) {
//         return;
//     }
//     led_state[led] = value;
//     // flylog("led %d state is %d \n",led,led_state[led]);
//     gpio_set_level(led_pin[led], value);
// }

void ledSet(led_t led, LED_STATE value)
{
    if (led >= LED_NUM ) {
        return;
    }
    led_state[led] = value;
    // flylog("led %d state is %d \n",led,led_state[led]);
    if(led == LED0)
    {
        gpio_set_level(led_pin[led], value);
    }
    else
    {
        mt[led-1].LED = value;
    }
}


void led_toggle(led_t led)
{
    if (led >= LED_NUM ) {
        return;
    }
    led_state[led] = !led_state[led];
    // flylog("led %d state is %d \n",led,led_state[led]);
    // gpio_set_level(led_pin[led], led_state[led]);
    ledSet(led,led_state[led]);
}

// void led_toggle(led_t led)
// {
//     if (led >= LED_NUM ) {
//         return;
//     }
//     led_state[led] = !led_state[led];
//     // flylog("led %d state is %d \n",led,led_state[led]);
//     gpio_set_level(led_pin[led], led_state[led]);
// }


bool get_led_state(led_t led)
{
    return led_state[led];
}

/*************
 * bit 0: led 0  state
 * bit 1: led 1  state
 * bit 2: led 2  state
 * bit 3: led 3  state
 * ***************/
char get_all_led_state(void)
{
    char state=0;
    for(int i=LED_NUM-1;i>=0;i--)
    {
        state = state<<1;
        if(led_state[i])
        {
            state = state | 0x1;
        }
    }
    return state;
}

/**************
 * set led all by state
 *  
 * **************/
void set_led_state(char state)
{
    char temp =0x01;
    for(int i=0;i<LED_NUM;i++)
    {
        if(state & temp)
            ledSet(i, 1);
        else
            ledSet(i, 0);
        temp=temp<<1;
    }
    // flylog("set state is %x ..................\n",state);
}
